/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: JointGroupCtrlData.cpp,v 1.10 2009-05-13 07:18:17 amaula Exp $
 *
 * \file
 * \brief Implementation for JointGroupCtrl interface data encoder/decoder.
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 */

#include "JointGroupCtrlData.hpp"
#include "binbag.h"
#include "owndebug.h"

//*****************************************************************************
using namespace MaCI::JointGroupCtrl;
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
CJointGroupCtrlData::CJointGroupCtrlData(void)
  :iValid(false),
   iCommandPtr(NULL),
   iJointSpeedPtrArray(),
   iJointPositionPtrArray(),
   iJointForcePtrArray(),
   iJointNumberPtr(),
   iJointInformatioHeaderPtr(NULL),
   iJointInformation_PayloadPtr(NULL),
   iEventTypesPtr(NULL),
   iJointGroupStatusPtr(NULL),
   iJointInfoElementCount(0)
{

}
//*****************************************************************************
      
CJointGroupCtrlData::~CJointGroupCtrlData(void)
{

}
//*****************************************************************************
           
CJointGroupCtrlData::CJointGroupCtrlData(const CJointGroupCtrlData &aJointGroupCtrlData)
  :iValid(false),
   iCommandPtr(NULL),
   iJointSpeedPtrArray(),
   iJointPositionPtrArray(),
   iJointForcePtrArray(),
   iJointNumberPtr(),
   iJointInformatioHeaderPtr(NULL),
   iJointInformation_PayloadPtr(NULL),
   iEventTypesPtr(NULL),
   iJointGroupStatusPtr(NULL),
   iJointInfoElementCount(0)
{
  *this = aJointGroupCtrlData;
}
//*****************************************************************************

CJointGroupCtrlData &CJointGroupCtrlData::operator=(const CJointGroupCtrlData &aJointGroupCtrlData)
{
  // Destroy the internal binbag (unconditional)
  DestroyInternalBinBag();
  
  // Create an internal BinBag instance
  if (CreateInternalBinBag(aJointGroupCtrlData.iBinBagContainer) == false) {
    dPrint(4,"WARNING: Failed to create internal binbag for object %p from object %p",
           this, &aJointGroupCtrlData);
  }
  
  // Do the decode on the buffer; it sets pointers and
  if (DecodeFrom() == false) {
    dPrint(4,"WARNING: Failed to Decode JointGroupCtrlData even though assignment operator used?");
  }

  return *this; 
}
//*****************************************************************************

bool CJointGroupCtrlData::DecodeFrom(gim::binbag::CBinBag *aBinBag)
{
  bool result = true;
  const gim::binbag::TBinBlob *b = NULL;
  int previous_type = -1;
  
  // Store pointer and test it for NullNess
  if (aBinBag != NULL)  
    iBinBagContainer = aBinBag;
  
  // Parse
  if (iBinBagContainer && iBinBagContainer->Rewind()) {
    while( (b = iBinBagContainer->GetNextBinary()) && result ) {
      switch(b->type) {
      case KTypeCommand:{
	if (iCommandPtr == NULL) {
       
          iCommandPtr = reinterpret_cast<const TCommand *>(b->data);
        } else {
	
	  result = false;
	}
	break;
      }
      case KTypeJointSpeed:{
        if(b->size == sizeof(TJointSpeed)){
          iJointSpeedPtrArray.push_back(reinterpret_cast<const TJointSpeed *>(b->data));
        }else{

 return false;
        }
        break;

      }
      case KTypeJointPosition:{
        if(b->size == sizeof(TJointPosition)){
          iJointPositionPtrArray.push_back(reinterpret_cast<const TJointPosition *>(b->data));
        }else{

 return false;
        }
        break;


      }
      case KTypeJointForce:{
        if(b->size == sizeof(TJointForce)){
          iJointForcePtrArray.push_back(reinterpret_cast<const TJointForce *>(b->data));
        }else{

 return false;
        }
        break;

      }
      case KTypeJointInformatioHeader:{
	if (iJointInformatioHeaderPtr == NULL) {
          iJointInformatioHeaderPtr = reinterpret_cast<const TJointInformationHeader*>(b->data);
        } else {

	  result = false;
	}

	break;
      }
      case KTypeJointInformation:{
	if (iJointInformation_PayloadPtr == NULL && iJointInformatioHeaderPtr != NULL &&
	    (previous_type == KTypeJointInformatioHeader)) {
	  iJointInformation_PayloadPtr = reinterpret_cast<const TJointInformation_Payload *>(b->data);
	  iJointInfoElementCount++;
	} else if (iJointInformatioHeaderPtr != NULL && 
		   (previous_type == KTypeJointInformatioHeader || previous_type == KTypeJointInformation)) {
	  iJointInfoElementCount ++;
	  
	} else{

	  result = false;

	}
	  break;    
      }
      case KTypeJointNumber:{
        
        if(iJointNumberPtr == NULL){
          iJointNumberPtr = reinterpret_cast<const TJointNumber *>(b->data);
        }else{
         return false;
        }
        break;

      }
      case KTypeEventType:{
	if (iEventTypesPtr == NULL) {
          iEventTypesPtr = reinterpret_cast<const TEventTypes *>(b->data);
        } else {
	 
	  result = false;
	}
	break;
      }

        
      case KTypeJointGroupStatus:{
	if (iJointGroupStatusPtr == NULL) {
          iJointGroupStatusPtr = reinterpret_cast<const TJointGroupStatus*>(b->data);
        } else {
         
	  result = false;
	}

	break;
      }

   
      default:{
	// Unknown Type in BinBag. Do nothing.
	break;
      }
	
	
      }
      // Store previous type
      previous_type = b->type;
    }
  }else{

    result = false;
  }
  
  // Store the result
  SetReadOnlyContainer( (iValid = result) );
  
  return result;
}
//*****************************************************************************

void CJointGroupCtrlData::Reset()
{
  CMaCIData::Reset();

  iValid = false;
  iCommandPtr = NULL;
  iJointPositionPtrArray.clear();
  iJointForcePtrArray.clear();
  iJointInformatioHeaderPtr = NULL;
  iJointInformation_PayloadPtr =NULL;
  iJointGroupStatusPtr = NULL;
  iEventTypesPtr = NULL;
  iJointSpeedPtrArray.clear();
  
}
//*****************************************************************************

void CJointGroupCtrlData::Print(const int level) const
{
  const gim::binbag::TBinBlob *b = NULL;
  // Parse
  if (iBinBagContainer && iBinBagContainer->Rewind()) {
    while( (b = iBinBagContainer->GetNextBinary())) {
      switch(b->type) {
      case KTypeCommand: {
        const TCommand *cp = reinterpret_cast<const TCommand *>(b->data);
        dPrint(level,"TCommand: %d",
               cp->cmd);
        break;
      }
      case KTypeJointSpeed:{
        const TJointSpeed *js = reinterpret_cast<const TJointSpeed *>(b->data);
        dPrint(level,"TJointSpeed: speed %f, jointNumber %d",
               js->speed,
	       js->jointNumber);

	break;
      }
      case KTypeJointPosition:{
	const TJointPosition *js = reinterpret_cast<const TJointPosition *>(b->data);
        dPrint(level,"TJointPosition: position %f, jointNumber %d",
               js->position,
	       js->jointNumber);

	
	break;
      }
      case KTypeJointForce:{
	const TJointForce *js = reinterpret_cast<const TJointForce *>(b->data);
        dPrint(level,"TJointForce: force %f, jointNumber %d",
               js->force,
	       js->jointNumber);

	
	break;
      }
      case KTypeJointInformatioHeader:{
	const TJointInformationHeader *jih = reinterpret_cast<const TJointInformationHeader *>(b->data);
        dPrint(level,"TJointInformationHeader: number of joints %d",
               jih->numberOfJoints);

	break;
      }
      case KTypeJointInformation:{
	const TJointInformation_Payload *jip = reinterpret_cast<const TJointInformation_Payload *>(b->data);
        dPrint(level,"TJointInformation_Payload: name %s, jointnumber %d, type %d, cancontrolspeed %d, maxSpeed%f, canControlposition %d, minpos %f, maxPos %f, canControlForce %d, minForce %f, maxForce %f\n",
               jip->name,
               jip->jointNumber ,
               jip->jointControlType,
               jip->canControlSpeed ,
               jip->maxSpeed,
               jip->canControlPosition,
               jip->minPosition,
               jip->maxPosition,
               jip->canControlForce,
               jip->minForce,
               jip->maxForce);
        
        
       
	break;
      }
      case KTypeJointNumber:{
	const TJointNumber *jn = reinterpret_cast<const TJointNumber *>(b->data);
	dPrint(level,"TJointNumber: number %d",
	       jn->number);
	break;
      }
      case KTypeJointGroupStatus:{
        const TJointGroupStatus *jgs = reinterpret_cast<const TJointGroupStatus *>(b->data);
	dPrint(level,"TJointGroupStatus: staus %d",
	       jgs->status);
	break;
        
      }
      case KTypeEventType:{
	const TEventTypes *et = reinterpret_cast<const TEventTypes *>(b->data);
	dPrint(level,"TEventTypes: types %d",
	       et->continuousTypes);
	break;
      }

      default:
	dPrint(level,"Unknown type! (%d)", b->type);
	// Unknown Type in BinBag. Do nothing.
	break;
      }
    }
  }
}
//*****************************************************************************

bool CJointGroupCtrlData::SetCommand(const TCommand &aCmd)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iCommandPtr == NULL) {
    // Use the handy Macro found from MaCIData
    ADDELEMENT(TCommand, KTypeCommand, iCommandPtr, aCmd);
    result = true;
  }
  return result;
}
//*****************************************************************************

bool CJointGroupCtrlData::SetSpeed(const TJointSpeed &aSpeed)
{

  bool result = false;
  if (!IsReadOnlyContainer() ){
    
    
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeJointSpeed, 
                                  (const char *)&aSpeed, 
                                  sizeof(TJointSpeed));
    
    // Typecast
    const TJointSpeed *speed = reinterpret_cast<const TJointSpeed *>(b->data); 
    
    // Store.
    iJointSpeedPtrArray.push_back(speed);
    SetLastElementType(KTypeJointSpeed);
    result = true;  
  }
  
 
  return result;

 

}

//*****************************************************************************

bool CJointGroupCtrlData::SetPosition(const TJointPosition &aPosition)
{
  bool result = false;
  if (!IsReadOnlyContainer()){
    
    
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeJointPosition, 
                                  (const char *)&aPosition, 
                                  sizeof(TJointPosition));
    
    // Typecast
    const TJointPosition *position = reinterpret_cast<const TJointPosition *>(b->data); 
    
    // Store.
    iJointPositionPtrArray.push_back(position);
    SetLastElementType(KTypeJointPosition);
    result = true;  
  }
  
 
  return result;

}
//*****************************************************************************

bool CJointGroupCtrlData::SetForce(const TJointForce &aForce)
{
  bool result = false;
  if (!IsReadOnlyContainer()){
    
    
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeJointForce, 
                                  (const char *)&aForce, 
                                  sizeof(TJointForce));
    
    // Typecast
    const TJointForce *force = reinterpret_cast<const TJointForce *>(b->data); 
    
    // Store.
    iJointForcePtrArray.push_back(force);
    SetLastElementType(KTypeJointForce);
    result = true;  
  }
  
 
  return result;

}
//*****************************************************************************
 
bool CJointGroupCtrlData::SetJointInformationHeader(const TJointInformationHeader &aHeader)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iJointInformatioHeaderPtr == NULL) {
    // Use the handy Macro found from MaCIData
    ADDELEMENT(TJointInformationHeader, KTypeJointInformatioHeader, iJointInformatioHeaderPtr, aHeader);
    result = true;
  }
  return result;

}
//*****************************************************************************

bool CJointGroupCtrlData::SetJointInformation(const TJointInformation_Payload &aInfo)
{
   bool result = false;
   if (!IsReadOnlyContainer() && 
       iJointInformatioHeaderPtr != NULL && 
       iJointInfoElementCount < (int)iJointInformatioHeaderPtr->numberOfJoints &&
       (GetLastElementType() == KTypeJointInformatioHeader || GetLastElementType() == KTypeJointInformation)) {
     // 1. Not read-only
     // 2. JointInformationHeader is set
     // 3. Added element count is under header's numberOfJoints
     // 4. Element is in sequence (previous is either JointInformation or JointInformationHeader)
    
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeJointInformation, (const char *)&aInfo, 
				  sizeof(TJointInformation_Payload));
    
    if (iJointInformation_PayloadPtr == NULL) {
      iJointInformation_PayloadPtr = reinterpret_cast<const TJointInformation_Payload *>(b->data);
    }

    // Increment element count
    ++iJointInfoElementCount;

    // If added element count matches the set header count, set iValid == true
    if ((int)iJointInformatioHeaderPtr->numberOfJoints == iJointInfoElementCount) iValid = true;

    // Mark last element
    SetLastElementType(KTypeJointInformation);

    result = true;
  }
  return result;

}
//*****************************************************************************

bool CJointGroupCtrlData::SetJointNumber(const TJointNumber &aNumber)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iJointNumberPtr == NULL) {
    // Use the handy Macro found from MaCIData
    ADDELEMENT(TJointNumber, KTypeJointNumber, iJointNumberPtr, aNumber);
    result = true;
  }
  return result;

}
//*****************************************************************************

bool CJointGroupCtrlData::SetJointGroupStatus(const TJointGroupStatus &aStatus)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iJointGroupStatusPtr == NULL) {
    // Use the handy Macro found from MaCIData
    ADDELEMENT(TJointGroupStatus, KTypeJointGroupStatus, iJointGroupStatusPtr, aStatus);
    result = true;
  }
  return result;


}
//*****************************************************************************

bool CJointGroupCtrlData::SetEventTypes(const TEventTypes &aEventTypes)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iEventTypesPtr == NULL) {
    // Use the handy Macro found from MaCIData
    ADDELEMENT(TEventTypes, KTypeEventType, iEventTypesPtr, aEventTypes);
    result = true;

  }
  return result;
}
//*****************************************************************************

const TCommand *CJointGroupCtrlData::GetCommand(void) const
{
  return iCommandPtr;

}
//*****************************************************************************

const TJointSpeed *CJointGroupCtrlData::GetJointSpeed(void) const
{
  if(iJointSpeedPtrArray.size() != 0){
    return iJointSpeedPtrArray[0];
  }else{
    return NULL;
  }

}
//*****************************************************************************

const TJointSpeedPtrArray &CJointGroupCtrlData::GetJointSpeedPtrArray(void) const
{

  return iJointSpeedPtrArray;
}
//*****************************************************************************

const TJointPosition *CJointGroupCtrlData::GetJointPosition(void) const
{
  if(iJointForcePtrArray.size() != 0){
    return iJointPositionPtrArray[0];
  }else{
    return NULL;
  }

}
//*****************************************************************************

const TJointPositionPtrArray &CJointGroupCtrlData::GetJointPositionPtrArray(void) const
{

  return iJointPositionPtrArray;
}
//*****************************************************************************

const TJointForce *CJointGroupCtrlData::GetJointForce(void) const
{
  if(iJointForcePtrArray.size() != 0){
    return iJointForcePtrArray[0];
  }else{
    return NULL;
  }

}
//*****************************************************************************

const TJointForcePtrArray &CJointGroupCtrlData::GetJointForcePtrArray(void) const
{

  return iJointForcePtrArray;
}
//*****************************************************************************

const TJointInformationHeader *CJointGroupCtrlData::GetJointInformationHeader(void) const
{
  return iJointInformatioHeaderPtr;

}
//*****************************************************************************

const TJointInformation_Payload *CJointGroupCtrlData::GetJointInformation(const int aIndex) const
{
  const TJointInformation_Payload *elem = NULL;
  
  if (iValid) {
    if (aIndex >= 0 && 
	aIndex < iJointInfoElementCount && 
	iJointInformation_PayloadPtr != NULL) {
      
      const char *cptr = (const char *)iJointInformation_PayloadPtr;
      cptr += aIndex * (sizeof(TJointInformation_Payload) + sizeof(gim::binbag::TBinBlob));
      
      // Now, typecast the address to Element.
      elem = (const TJointInformation_Payload *)cptr;
    }else{
      dPrint(1,"index or failed, or ptr == NULL");
    }
  }else{
    dPrint(1,"not valid!");
  }
  return elem;

}
//*****************************************************************************

const TJointNumber *CJointGroupCtrlData::GetJointNumber() const
{
  return iJointNumberPtr;

}
//*****************************************************************************

const TJointGroupStatus *CJointGroupCtrlData::GetJointGroupStatus() const
{

  return iJointGroupStatusPtr;
}
//*****************************************************************************

const TEventTypes *CJointGroupCtrlData::GetEventTypes() const
{
  return iEventTypesPtr;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

#ifdef COMPILE_JOINTGROUPCTRLDATA_STANDALONE_MAIN


// Assert sequences verify that the container is working correctly
// (== The data read matches the data wrote in)
#include <assert.h>

int main(void)
{
  // Init Debug lib
  debugInit();
  debugSetGlobalDebugLvl(4);
  using namespace MaCI::JointGroupCtrl;
  gim::binbag::CBinBag bb;
  CJointGroupCtrlData A;
  CJointGroupCtrlData B;
  printf("Carry out Encoding/Decoding test.\n");

  printf("Encoding...\n");
  // Do Encoding 
  A.EncodeTo(&bb);

  printf("Setting Command KCommandSetGroupSpeed(%d)\n",KCommandSetGroupSpeeds);
  A.SetCommand(TCommand(KCommandSetGroupSpeeds));
	       
  int jointNumber = 3;
  float speed = 0.3;

  printf("Setting TJointSpeed(%f, %d)\n",speed, jointNumber);
  A.SetSpeed(TJointSpeed(speed,jointNumber));

  speed = 0.2; jointNumber =2;
  printf("Setting TJointSpeed(%f, %d)\n",speed, jointNumber);
  A.SetSpeed(TJointSpeed(speed,jointNumber));

  jointNumber = 2;
  float angle = 3.14;
  printf("Setting TJointPosition(%f, %d)\n",angle, jointNumber);
  A.SetPosition(TJointPosition(angle,jointNumber));

  jointNumber = 1;
  float force = 4;
  printf("Setting TJointForce (%f,%d)\n",force,jointNumber);
  A.SetForce(TJointForce(force,jointNumber));

  //Just 
  TEventTypes eventTypes;
  eventTypes.SetEventType(KTypeJointPosition);  
  eventTypes.SetEventType(KTypeJointSpeed);
  printf("Setting TEventTypes (bitmask %d)\n",eventTypes.continuousTypes);
  A.SetEventTypes(eventTypes);

  printf("IsSet (pos %d) (speed %d) (force %d)\n", eventTypes.IsSet(KTypeJointSpeed),eventTypes.IsSet(KTypeJointPosition),eventTypes.IsSet(KTypeJointForce));
  unsigned int numberOfJoints = 3;
  printf("Setting TJointInformationHeader(%d)\n", numberOfJoints);
  A.SetJointInformationHeader(TJointInformationHeader(numberOfJoints));
  
  
  std::string name = "Joint nro.";
  std::string finalName;
  for(unsigned int i = 0; i< numberOfJoints; i++){
    char idNumberStr[32];
    sprintf(idNumberStr,"%d",i);
    finalName = name + std::string(idNumberStr);

    printf("Setting %d. TJointInformation_Payload(%s, %d(angular),cancontrolspeed(%d),maxSpeed %f,canControlposition %d,minpos %f, maxPos %f,canControlForce %d, minForce %f, maxForce %f)\n",
	   i+1,
	   finalName.c_str(),
           KJointControlAngular,
           1,
           (float)1.0,
           1,
           -M_PI/(float)(i+1),
           M_PI/(float)(i+1),
           1,
           (float)(0.1+i),
           (float)(1.2+i));

    TJointInformation_Payload info(i,KJointControlAngular,1,1.0,1,-M_PI/(i+1),M_PI/(i+1), 1,(float)(0.1+i),(float)(1.2+i));
    info.SetName(finalName);
    
    A.SetJointInformation(info);

  }

  printf("Setting TJointGroupStatus(Status OPEN = %d)\n", KJointGroupStatusOpen );
  A.SetJointGroupStatus(TJointGroupStatus(KJointGroupStatusOpen));
  printf("Decoding...\n");
  
  // Do Decoding
  B.DecodeFrom(&bb);
  printf("Decode: Command = (%d)\n", 
	 B.GetCommand()->cmd);

  TJointSpeedPtrArray sArray = B.GetJointSpeedPtrArray();
 
  for(EACH_IN_i(sArray)){
    printf("Decode: JointSpeed = (%f,%d)\n",
           (*i)->speed,
           (*i)->jointNumber);
  }


  TJointPositionPtrArray pArray = B.GetJointPositionPtrArray();
  for(EACH_IN_i(pArray)){
    printf("Decode: JointPosition = (%f,%d)\n",
           (*i)->position,
           (*i)->jointNumber);
  }


  TJointForcePtrArray fArray = B.GetJointForcePtrArray();
  for(EACH_IN_i(fArray)){
    printf("Decode: JointForce = (%f,%d)\n",
           (*i)->force,
           (*i)->jointNumber);
  }

  printf("Decode EventTypes = (bitmask %d)\n",
         B.GetEventTypes()->continuousTypes);

  printf("Decode JointInformationHeader = (%d)\n",
	 B.GetJointInformationHeader()->numberOfJoints);

  for(unsigned int i = 0; i<numberOfJoints;i++){
    printf("Decode JointInformation = (%s, number %d, type:%d, cancontrolspeed %d, maxSpeed%f,canControlposition %d,minpos %f, maxPos %f, cantControlForce %d, minForce %f, maxForce %f)\n",
	   B.GetJointInformation(i)->name,
           B.GetJointInformation(i)->jointNumber ,
           B.GetJointInformation(i)->jointControlType,
	   B.GetJointInformation(i)->canControlSpeed ,
           B.GetJointInformation(i)->maxSpeed,
	   B.GetJointInformation(i)->canControlPosition,
           B.GetJointInformation(i)->minPosition,
           B.GetJointInformation(i)->maxPosition,
           B.GetJointInformation(i)->canControlForce,
           B.GetJointInformation(i)->minForce,
           B.GetJointInformation(i)->maxForce);

  }
  printf("Decode JointGroupStatus = (%d)\n",
	 B.GetJointGroupStatus()->status);

  CJointGroupCtrlData C(B); 
  printf("Trying the copy constructor..\n");
  printf("Decode: Command = (%d)\n", 
	 C.GetCommand()->cmd);
  
  sArray = C.GetJointSpeedPtrArray();
  for(EACH_IN_i(sArray)){
    printf("Decode: JointSpeed = (%f,%d)\n",
           (*i)->speed,
           (*i)->jointNumber);
  }


  pArray = C.GetJointPositionPtrArray();
  for(EACH_IN_i(pArray)){
    printf("Decode: JointPosition = (%f,%d)\n",
           (*i)->position,
           (*i)->jointNumber);
  }


  fArray = C.GetJointForcePtrArray();
  for(EACH_IN_i(fArray)){
    printf("Decode: JointForce = (%f,%d)\n",
           (*i)->force,
           (*i)->jointNumber);
  }

  printf("Decode EventTypes = (bitmask %d)\n",
         C.GetEventTypes()->continuousTypes);

  printf("Decode JointInformationHeader = (%d)\n",
	 C.GetJointInformationHeader()->numberOfJoints);
  for(unsigned int i = 0; i<numberOfJoints;i++){
    printf("Decode JointInformation = (%s, number %d, type:%d, cancontrolspeed %d, maxSpeed%f,canControlposition %d,minpos %f, maxPos %f, cantControlForce %d, minForce %f, maxForce %f)\n",
	   C.GetJointInformation(i)->name,
           C.GetJointInformation(i)->jointNumber ,
           C.GetJointInformation(i)->jointControlType,
	   C.GetJointInformation(i)->canControlSpeed ,
           C.GetJointInformation(i)->maxSpeed,
	   C.GetJointInformation(i)->canControlPosition,
           C.GetJointInformation(i)->minPosition,
           C.GetJointInformation(i)->maxPosition,
           C.GetJointInformation(i)->canControlForce,
           C.GetJointInformation(i)->minForce,
           C.GetJointInformation(i)->maxForce);

  }
 
  printf("Decode JointGroupStatus = (%d)\n",
	 C.GetJointGroupStatus()->status);

  CJointGroupCtrlData D = C;
  printf("Trying the = operator\n");
  printf("Decode: Command = (%d)\n", 
	 D.GetCommand()->cmd);


 sArray = C.GetJointSpeedPtrArray();
  for(EACH_IN_i(sArray)){
    printf("Decode: JointSpeed = (%f,%d)\n",
           (*i)->speed,
           (*i)->jointNumber);
  }


  pArray = C.GetJointPositionPtrArray();
  for(EACH_IN_i(pArray)){
    printf("Decode: JointPosition = (%f,%d)\n",
           (*i)->position,
           (*i)->jointNumber);
  }


  fArray = C.GetJointForcePtrArray();
  for(EACH_IN_i(fArray)){
    printf("Decode: JointForce = (%f,%d)\n",
           (*i)->force,
           (*i)->jointNumber);
  }

   printf("Decode JointInformationHeader = (%d)\n",
	 D.GetJointInformationHeader()->numberOfJoints);

  printf("Decode EventTypes = (bitmask %d)\n",
         D.GetEventTypes()->continuousTypes);

  for(unsigned int i = 0; i<numberOfJoints;i++){
    printf("Decode JointInformation = (%s, number %d, type:%d, cancontrolspeed %d, maxSpeed%f,canControlposition %d,minpos %f, maxPos %f, cantControlForce %d, minForce %f, maxForce %f)\n",
	   D.GetJointInformation(i)->name,
           D.GetJointInformation(i)->jointNumber ,
           D.GetJointInformation(i)->jointControlType,
	   D.GetJointInformation(i)->canControlSpeed ,
           D.GetJointInformation(i)->maxSpeed,
	   D.GetJointInformation(i)->canControlPosition,
           D.GetJointInformation(i)->minPosition,
           D.GetJointInformation(i)->maxPosition,
           D.GetJointInformation(i)->canControlForce,
           D.GetJointInformation(i)->minForce,
           D.GetJointInformation(i)->maxForce);

  }
  

  printf("Decode JointGroupStatus = (%d)\n",
	 D.GetJointGroupStatus()->status);
  return 0;


}




#endif
